#include "pid.h"

// 初始化PID
void PID_Init(PID_TypeDef *pid, float kp, float ki, float kd) {
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->integral = 0.0f;
    pid->prevError = 0.0f;
    pid->output = 0.0f;
}

// PID计算
float PID_Compute(PID_TypeDef *pid, float currentValue,float setpoint) {
    float error = setpoint - currentValue;
    pid->integral += error;
    float derivative = error - pid->prevError;

    pid->output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;

    // 输出限幅
    if (pid->output > pid->maxOutput) {
        pid->output = pid->maxOutput;
    } else if (pid->output < pid->minOutput) {
        pid->output = pid->minOutput;
    }
    pid->prevError = error;
    return pid->output;
}

// 云台PID计算
// error: 误差值
// currentValue: 当前值
// 返回值: 输出值
int P_Yuntai(PID_TypeDef *pid,int error,int currentValue){
    int output;
    static int error_sum=0;
    static int error_last=0;
    error_sum+=error;
    if(error_sum>1000) error_sum=1000; //限制积分范围
    if(error_sum<-1000) error_sum=-1000;
    int error_diff=error-error_last;
    error_last=error;
    output+=error_sum*pid->ki+error_diff*pid->kd+pid->kp*error+currentValue; //PID计算
    if(output<830) output=830; //限制输出范围833
    if(output>2160) output=2160;//2167
    return output;
}



